#ifndef TARGETTRACK_H
#define TARGETTRACK_H

#include <semaphore.h>

#include <vector>
#include <map>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "timestamp.h"
#include "cameradaemon.h"
#include "searchablequeue.h"

class TargetTrack
{
public:
    TargetTrack();
    ~TargetTrack();
    bool getImage(cv::Mat &img, TimeStamp &ts);
    bool getTrackImage(cv::Mat &img, TimeStamp &ts);
    bool getTrackResult(cv::Mat &img);
    void getTrackResult(TimeStamp &ts, cv::Rect &area, cv::Point2f &center);
    void setCamera(CameraDaemon* cam);
    void nextFrame();
    void startTarck(TimeStamp &ts, int x1, int y1, int x2, int y2);
    static bool selectMask(cv::Mat mask, cv::Rect area, cv::Mat &out, cv::Point2f* center);
    void motionEstimate(TimeStamp last_ts,
                        TimeStamp cur_ts,
                        cv::Rect last_area,
                        cv::Rect &estimated_area);
    void updateMotionInfo(std::vector<cv::KeyPoint> points, TimeStamp last_ts, TimeStamp cur_ts);

     bool m_status;

protected:
    bool findSiftKeypoints(cv::Mat &obj, std::vector<cv::Point2f> &keypoints, int &count);
    void setTarget(cv::Point2f top_left, cv::Point2f bottom_right);

private:


    sem_t m_frame_mutex;
    cv::Mat m_prev;
    TimeStamp m_prev_ts;
    cv::Mat m_cur;
    TimeStamp m_cur_ts;
    CameraDaemon* m_cam;

    sem_t m_track_mutex;

    SearchableQueue<unsigned int, cv::Mat> m_track_imgs;

    cv::Mat m_track_img;
    TimeStamp m_track_img_ts;
    cv::Point2f m_top_left;
    cv::Point2f m_bottom_right;
    cv::Rect m_search_area;
    cv::Point2f m_obj_center;
    bool m_track_new;

    std::vector<unsigned char> m_cur_keypoints_status;
    std::vector<float> m_cur_keypoints_err;
    std::vector<cv::Point2f> m_cur_keypoints;
    std::vector<cv::Point2f> m_track_keypoints;
    double m_velocity_x;
    double m_velocity_y;

};

#endif // TARGETTRACK_H
